!
| Note: This tutorial assumes basic familiarity with the OUR robot and MoveGroup class.. This tutorial assumes that you have completed the previous tutorials: http://wiki.ros.org/Robots/OUR/MoveIt%20Setup%20Assistant%20for%20OUR%20robot. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Plan a trajectory to control an OUR robot motion in Rviz.
Description: Use the MoveGroup class for planning trajectories to control OUR_robot motion in Rviz.Keywords: OUR MoveIt! Rviz trajectory
Tutorial Level: BEGINNER
Next Tutorial: Use the MoveGroup class for planning trajectories to control OUR_robot motion the real world. Plan a trajectory to control an OUR robot motion in real world
Contents
Pre-requisites
1. Already generate the necessary configure file for OUR the robot using MoveIt! setup assistant.
2. Having the basic for MoveGroup class.
Use the Move Group Interface to plan a trajectory sending to the topic of "moveJ"
We use the MoveGroup class to set joint or pose goal,creating motion plans, decomposing plans and sending the positions message to the topic of "moveJ".
(1) The MoveGroup class can be easily setup using just the name of the group you would like to control and plan for.
1 moveit::planning_interface::MoveGroup group("joint_group");
(2) Getting Basic Information
(3) First get the current set of joint values for the group.
- let’s modify one of the joints, plan to the new joint space goal. we call the planner to compute the plan.
1 std::vector<double> group_variable_values;
2 group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
3
4 group_variable_values[0] = 0;
5 group_variable_values[1] = 20*M_PI/180.0;
6 group_variable_values[2] = -70*M_PI/180.0;
7 group_variable_values[3] = 0;
8 group_variable_values[4] = -90*M_PI/180.0;
9 group_variable_values[5] = 0;
10 group.setJointValueTarget(group_variable_values);
11 moveit::planning_interface::MoveGroup::Plan my_plan;
12 bool success = group.plan(my_plan);
(4) Discompose the trajectory and publish to the topic named "moveJ"
1 std_msgs::Float32MultiArray joints;
2 joints.data.resize(6);
3 ros::NodeHandle nh;
4 ros::Publisher command_pub = nh.advertise<std_msgs::Float32MultiArray> ("moveJ", 1000);
5 ros::Rate loop_rate(20);//Hz
6 int j =0;
7 while(ros::ok())
8 {
9 if( j != my_plan.trajectory_.joint_trajectory.points.size()) //or return and again
10 {
11 for(j=0; j<my_plan.trajectory_.joint_trajectory.points.size(); j++)
12 {
13 for(int i=0; i<6; i++)
14 {
15 joints.data[i]=my_plan.trajectory_.joint_trajectory.points[j].positions[i];
16 ROS_INFO("send ponits[%d]positions[%d] %f",j,i,joints.data[i]);
17 }
18 command_pub.publish(joints);
19 loop_rate.sleep();
20 }
21 }
22 }
Write the node to subscribe the "moveJ" topic and show the motion trajectory in Rviz
(1) Subscribe the "moveJ" topic
(2) The callback function is used to show the motion in Rviz.
1 void chatterCallback(const std_msgs::Float32MultiArray::ConstPtr &msg)
2 {
3 ROS_INFO("[%f,%f,%f,%f,%f,%f]",
4 msg->data[0],msg->data[1],msg->data[2],msg->data[3],msg->data[4],msg->data[5]);
5 for(int i=0; i<6; i++)
6 {
7 group_variable_values[i] = msg->data[i];
8 }
9 ptr_group->setJointValueTarget(group_variable_values);
10 ptr_group->asyncMove();
11 }
